﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Wilke.Interactive.Drone.Control;
using Wilke.Interactive.Drone.Control.Enumerations;
using System.Net;
using MindDrone.Common;
using System.Diagnostics;

namespace ARDroneConnector
{
    /// <summary>
    /// ArDroneConnector wrapper permet d'initier et controller l'AR Drone
    /// </summary>
    public class ArDroneConnector
    {
        private const float THROTTLE_MINIMUM = -1;
        private const float THROTTLE_MAXIMUM = 1;

        public ConnectorStatus Status { get; set; }
        private String Ssid { get; set; }
        //public String PlayerIP { get; set; }
        //public String DroneIP { get; set; }²
        private DroneController ArdController { get; set; }
        //private string DroneConnectionStatus { get; set; }

        /// <summary>
        /// EventHandler qui notifie quand le status a change
        /// </summary>
        public event EventHandler<DroneEventArgs> OnStatusChanged;
        
        /// <summary>
        /// EventHandler qui notifie quand le status a change
        /// </summary>
        public event EventHandler<DroneEventArgs> OnImageChanged;

        /// <summary>
        /// Constructeur
        /// </summary>
        /// <param name="_Ssid"></param>
        public ArDroneConnector(String _Ssid)
        {
            this.Ssid = _Ssid;
            //this.PlayerIP = _PlayerIP;
            //this.DroneIP = _DroneIP;

            DroneControllerConfiguration droneControllerConfiguration = new DroneControllerConfiguration();

            droneControllerConfiguration.NetworkIdentifier = Ssid;
            //droneControllerConfiguration.DroneIpAddress = DroneIP;
            //droneControllerConfiguration.GuestIpAddress = PlayerIP;
            droneControllerConfiguration.NavigationDataPort = 5554;
            droneControllerConfiguration.VideoStreamPort = 5555;
            droneControllerConfiguration.CommandPort = 5556;
            droneControllerConfiguration.ControlInfoPort = 5559;
            droneControllerConfiguration.EnableATCommandThread = true;
            droneControllerConfiguration.EnableNavigationDataThread = true;
            droneControllerConfiguration.EnableVideoStreamThread = true;
            droneControllerConfiguration.EnableControlInfoThread = true;
            droneControllerConfiguration.EnableInputFeedback = true;

            //droneControllerConfiguration.RollThrottle = 0;
            //droneControllerConfiguration.PitchThrottle = 0;
            //droneControllerConfiguration.HeightThrottle = 30;
            //droneControllerConfiguration.YawThrottle = 0;

            ArdController = new DroneController(droneControllerConfiguration); 
            ArdController.SetIndoorConfiguration();
            ArdController.OnNotifyVideoMessage += new EventHandler<VideoNotificationEventArgs>(ArdController_OnNotifyVideoMessage);
            ArdController.OnConnectionStatusChanged += new EventHandler<ConnectionStatusChangedEventArgs>(ArdController_OnConnectionStatusChanged);
            Status = ConnectorStatus.Closed;
        }
        public static bool IsFirst = true;

        public void Connect()
        {
            ArdController.Connect();
        }

        void ArdController_OnConnectionStatusChanged(object sender, ConnectionStatusChangedEventArgs e)
        {
            Status = CastConnectorStatus(e.ConnectionStatus);
            //DroneConnectionStatus = ArdController.ConnectionStatus.ToString();
            //Trace.WriteLine(DroneConnectionStatus);

            if (OnStatusChanged != null)
            {
                OnStatusChanged(this, new DroneEventArgs(CastConnectorStatus(ArdController.ConnectionStatus)));
            }
        }
      
        public bool TakeOff()
        {
            if (ArdController != null && ArdController.ConnectionStatus == ConnectionStatus.Open)
            {
                ArdController.SetFlatTrim();
                ArdController.StartEngines();
                return true;
            }
            return false;
        }

        public void Land()
        {
            if (ArdController != null && ArdController.ConnectionStatus == ConnectionStatus.Open)
            {
                ArdController.StopEngines();
            }
        }

        public void Disconnect()
        {
            if (ArdController != null && ArdController.ConnectionStatus == ConnectionStatus.Open)
            {
                ArdController.Disconnect();
            }
        }

        public void StartVideo()
        {
            if (ArdController != null && ArdController.ConnectionStatus == ConnectionStatus.Open)
                ArdController.StartRecordVideo();
        }
       
        void ArdController_OnNotifyVideoMessage(object sender, VideoNotificationEventArgs e)
        {
            if (OnImageChanged != null)
                OnImageChanged(this, new DroneEventArgs(e.CurrentImage));
        }
        /// <summary>
        /// Bouger le drone
        /// </summary>
        /// <param name="roll"> Valeur en <c>float</c> entre -1 et 1 mais conseille entre -0.1 et 0.1  gauche a droite</param>
        /// <param name="pitch"> Valeur en <c>float</c> entre -1 et 1 mais conseille entre -0.1 et 0.1 avancer ou reculer </param>
        /// <param name="height"> Valeur en <c>float</c> entre -1 et 1 mais conseille entre -0.1 et 0.1 monter ou descendre</param>
        /// <param name="yaw"> Valeur en <c>float</c> entre -1 et 1 mais conseille entre -0.1 et 0.1 faire la rotation (valeur negatif vers la gauche, valeur positif vers la droite)</param>
        public void Move(float roll, float pitch, float height, float yaw)
        {
            if (!ValidateValueThrottle(roll) || !ValidateValueThrottle(pitch) || !ValidateValueThrottle(height) || !ValidateValueThrottle(yaw))
                throw new Exception("Valeur doit etre entre -1 et 1");
            else
                ArdController.SetFlightParameters(roll, pitch, height, yaw);
        }

        /// <summary>
        /// Valide une valeur
        /// </summary>
        /// <param name="value">valeur a validee de type <c>float</c></param>
        /// <returns></returns>
        protected bool ValidateValueThrottle(float value)
        {
            if (value > ArDroneConnector.THROTTLE_MAXIMUM || value < ArDroneConnector.THROTTLE_MINIMUM)
                return false;
            else
                return true;
        }

        protected ConnectorStatus CastConnectorStatus(ConnectionStatus status)
        {
            ConnectorStatus Status;
            switch (status)
            {
                case ConnectionStatus.CloseRequested:
                    Status = ConnectorStatus.CloseRequested;
                    break;
                case ConnectionStatus.Closed:
                    Status = ConnectorStatus.Closed;
                    break;
                case ConnectionStatus.Error:
                    Status = ConnectorStatus.Error;
                    break;
                case ConnectionStatus.Initializing:
                    Status = ConnectorStatus.Initializing;
                    break;
                case ConnectionStatus.Open:
                    Status = ConnectorStatus.Open;
                    break;
                default:
                    Status = ConnectorStatus.NotSet;
                    break;
            }
            return Status;
        }


    }



}
